using System;
using System.Collections.Generic;
using LegoBotPrimaAbstractions;
using LegoBotPrimaAbstractions.Extensions;

namespace LegoBotPrima.Actions
{
    public class Rotate360BotAction : BotAction
    {
        private readonly SortedDictionary<DateTime, SensorData> _sensorData = new SortedDictionary<DateTime, SensorData>();
        private readonly List<SensorData> _openSpaceAngles = new List<SensorData>();
        private double _previousAngle = -1;
        private double _rotationSoFar;
        private double _waitToRotateAngle;
        private RotationDirection _rotationDirection;
        private bool _highRisk;
        private double _tickNumber;

        private static readonly object _synch_SensorData = new object();
        private static readonly object _synch_OnTimerTick = new object();

        public Rotate360BotAction()
        {
            PreDefinedRotationDirection = RotationDirection.Undefined;
            PreDefinedRotationAngle = 0;
        }


        public override void Initialize()
        {
            this._rotationDirection = PreDefinedRotationDirection == RotationDirection.Undefined 
                                        ? ChooseRandomDirection() 
                                        : PreDefinedRotationDirection;

            if (this._rotationDirection == RotationDirection.Clockwise) Workflow.GoRightSlow();
            else Workflow.GoLeftSlow();

            this._highRisk = MathUtil.RandomBooleanWithProbability(0.5);

            this._waitToRotateAngle = PreDefinedRotationAngle <= 0 ? MathUtil.RandomIntFromRange(90, 360) : PreDefinedRotationAngle;
        }



        public RotationDirection PreDefinedRotationDirection { get; set; }
        public double PreDefinedRotationAngle { get; set; }
        public double AvoidAngle { get; set; }


        public override void OnCompassChanged(double degrees, DateTime timeStamp)
        {
            if (!IsRunning) return;
            lock (_synch_SensorData)
            {
                this.AddSensorData(new SensorData { Angle = degrees, TimeStamp = timeStamp, Sensor = SensorType.Angle });
                this.CalculateDistanceDataAngle();
                if(this._previousAngle >= 0)
                {
                    this._rotationSoFar += Math.Abs(MathUtil.AnglesDiff(degrees, this._previousAngle));
                    if(this._rotationSoFar >= this._waitToRotateAngle)
                    {
                        if(this._openSpaceAngles.Count > 0)
                        {
                            var i = MathUtil.RandomIntFromRange(0, this._openSpaceAngles.Count - 1);
                            var j = 0;
                            while (++j < 3 && AvoidAngle != 0 && Math.Abs(MathUtil.AnglesDiff(AvoidAngle, degrees)) < 20)
                            {
                                i = MathUtil.RandomIntFromRange(0, this._openSpaceAngles.Count - 1);
                            }
                            this.StopAndGoToAngle(this._openSpaceAngles[i].Angle);
                            return;
                        }
                    }
                    if(this._rotationSoFar > 720 && this._openSpaceAngles.Count == 0)
                    {
                        this.StopAndWaitThenRotateAgain();
                        return;
                    }
                }
                this._previousAngle = degrees;
            }
        }



        public override void OnDistanceChanged(int distance, DateTime timeStamp)
        {
            if (!IsRunning) return;
            lock (_synch_SensorData)
            {

                if (this._highRisk && 
                    distance > Config.Instance.MinDistance * 1.5 && 
                    (AvoidAngle == 0 || Math.Abs(MathUtil.AnglesDiff(AvoidAngle, Workflow.RotationAngle)) > 20))
                {
                    RequestedNextAction = new ForwardBotAction();
                    Workflow.Stop();
                    Workflow.EndAction(this);
                    return;
                }

                this.AddSensorData(new SensorData { Angle = -1, Distance = distance, TimeStamp = timeStamp, Sensor = SensorType.Distance });
            }
        }

        public override void OnTimerTick(int tickNimber)
        {
            if(!IsRunning) return;
            lock (_synch_OnTimerTick)
            {
                _tickNumber++;
                if (_tickNumber % SecondsAsTicks(Config.Instance.MaxSecondsRotating) == 0)
                {
                    Console.WriteLine("Max rotating reached");
                    RequestedNextAction = new StepBackBotAction();
                    Workflow.Stop();
                    Workflow.EndAction(this);
                }
            }
        }


        #region Private Methods
        private void StopAndWaitThenRotateAgain()
        {
            RequestedNextAction = new WaitBotAction { AfterWaitAction = new Rotate360BotAction() };
            Workflow.Stop();
            Workflow.EndAction(this);
        }

        private void StopAndGoToAngle(double angle)
        {
            RequestedNextAction = new GoToAngleBotAction { Angle = angle };
            Workflow.Stop();
            Workflow.EndAction(this);
        }

        private static RotationDirection ChooseRandomDirection()
        {
            return MathUtil.RandomBooleanWithProbability(0.5)
                        ? RotationDirection.Clockwise
                        : RotationDirection.Counterclockwise;
        }

        private void AddSensorData(SensorData data)
        {
            var timeStamp = data.TimeStamp;
            while (this._sensorData.ContainsKey(timeStamp))
            {
                timeStamp = timeStamp.AddMilliseconds(1);
            }
            this._sensorData.Add(timeStamp, data);
        }

        private void CalculateDistanceDataAngle()
        {
            var distanceDataBetweenAngles = new LinkedList<SensorData>();
            SensorData previousAngle = null;
            foreach (var data in this._sensorData)
            {
                if (data.Value.Sensor == SensorType.Distance)
                {
                    // already defined
                    if (data.Value.Angle >= 0)
                    {
                        continue;
                    }
                    // we are looking for a distance between two angles, if there is no angle before - ignore it
                    if (previousAngle == null)
                    {
                        continue;
                    }
                    distanceDataBetweenAngles.AddLast(data.Value);
                }
                else
                {
                    if(previousAngle != null && distanceDataBetweenAngles.Count > 0)
                    {
                        foreach (var distance in distanceDataBetweenAngles)
                        {
                            if (this._rotationDirection == RotationDirection.Clockwise) distance.DefineAngleByClockwiseNeighbours(previousAngle, data.Value);
                            else distance.DefineAngleByCounterclockwiseNeighbours(previousAngle, data.Value);

                            if(distance.Angle >= 0 && distance.Distance > Config.Instance.MinDistance)
                            {
                                this._openSpaceAngles.Add(distance);
                            }
                        }
                        distanceDataBetweenAngles.Clear();
                    }
                    previousAngle = data.Value;
                }
            }
        }



        #endregion

    }
}